import serial
import time
import msvcrt
import random

print "Started Simulation at :", time.clock()

Strgt 	= chr(0x32)
Left 	= chr(0x31)
Right	= chr(0x33)
Stop	= chr(0x34)
RotL 	= chr(0x35)

GPS_SPort 	= 'COM10' #GPS PORT
GpsPort 	= serial.Serial(GPS_SPort  , 9600, timeout=0)

Bot_SPort 	=  'COM12' #BOT PORT
BotPort 	= serial.Serial(Bot_SPort, 9600, timeout=0)

SampleGpsMsg = '#BESTUTMA,USB2,0,50.5,FINESTEERING,1692,265564.400,80000000,2775,6576;SOL_COMPUTED,SINGLE,17,R,3280538.0557,369509.1114,11.4042,0.0000,WGS84,1.8986,2.4845,3.1442,"",0.000,0.000,13,11,11,11,0,02,0,11*5009c08e\n'
GpsMsgArray  = SampleGpsMsg.split(',')
NewGpsMsg = ''

CurrentGPSx = float(GpsMsgArray[13])
CurrentGPSy = float(GpsMsgArray[14])
CurrentGPSz = GpsMsgArray[15]
CurrentTime = float(GpsMsgArray[6])

InputKey = 's'
deltax   = 1
deltay   = 1
delay    = 0.1
while InputKey != 'q':


	BotIn 			= BotPort.read()
	CurrentTime 	= CurrentTime + delay
	GpsMsgArray[6]  = str(CurrentTime)
	if( BotIn == Strgt):
		CurrentGPSx = (CurrentGPSx + deltax )
	if( BotIn == Left ):
		CurrentGPSx = (CurrentGPSx + deltax )
		CurrentGPSy = (CurrentGPSy + deltay )
	if( BotIn == Right):
		CurrentGPSx = (CurrentGPSx + deltax )
		CurrentGPSy = (CurrentGPSy - deltay )
	SendGPSx  	= CurrentGPSx + 0.5*random.random()
	SendGPSy  	= CurrentGPSy + 0.5*random.random()
	GpsMsgArray[13] = str(SendGPSx) 
	GpsMsgArray[14] = str(SendGPSy)
	NewGpsMsg = ''
	

	
	print 'CurrentGPSx = ', SendGPSx, 'CurrentGPSy =', SendGPSy
	for i in range(0,len(GpsMsgArray)):
		if i != 31:
			NewGpsMsg = NewGpsMsg + GpsMsgArray[i] +','
		else:
			NewGpsMsg = NewGpsMsg + GpsMsgArray[i]
	GpsPort.write(NewGpsMsg)		
	time.sleep(delay)

	
print "Exit Receiver"